package Physics;

public class InlineFunctions {
	public static float transformToAxis(CollisionBox box, Vector3 axis)
	{
		/** separating axis test. Project the three axes of the box onto the given axis */
		return  box.m_halfSize.x * Math.abs(axis.dotProduct(box.getAxis(0))) +
		        box.m_halfSize.y * Math.abs(axis.dotProduct(box.getAxis(1))) +
		        box.m_halfSize.z * Math.abs(axis.dotProduct(box.getAxis(2)));
	}
	
	/** checks if two boxes overlap along the given axis
	 Uses the separating axis theorem to project the boxes
	 onto the axis and determine if there's overlap.
	 The toCenter parameter is used to pass in the vector
	 between the boxes' center points to avoid having
	 to recalculate each time */
	public static boolean overlapOnAxis(CollisionBox one, CollisionBox two, Vector3 axis, Vector3 toCenter)
	{
		/** Project the box onto the axis */
		float oneProject = transformToAxis(one, axis);
		float twoProject = transformToAxis(two, axis);
		
		/** Project this onto the axis */
		float distance = Math.abs(toCenter.dotProduct(axis));
		
		/** Do they overlap? */
		return (distance < oneProject + twoProject);
	}
	
	/** This function checks if the two boxes overlap
	 * along the given axis, returning the amount of overlap.
	 * The final parameter toCentre
	 * is used to pass in the vector between the boxes center
	 * points, to avoid having to recalculate it each time. */
	public static float penetrationOnAxis(CollisionBox one, CollisionBox two, Vector3 axis, Vector3 toCenter)
	{
		/** Project the half-size of one onto axis */
		float oneProject = transformToAxis(one, axis);
		float twoProject = transformToAxis(two, axis);
		
		/** Project this onto the axis */
		float distance = Math.abs(toCenter.dotProduct(axis));
		
		/** Return the overlap (i.e. positive indicates
		 * overlap, negative indicates separation). */
		return oneProject + twoProject - distance;
	}
	public static boolean tryAxis(CollisionBox one, CollisionBox two, Vector3 axis, Vector3 toCenter,int index,
			/** These values may be updated */
			float smallestPenetration, int smallestCase)
	{
		/** Make sure we have a normalized axis, and don't check almost parallel axes */
		if(axis.squareMagnitude() < 0.0001)
			return true;
		
		axis.normalize();
		
		float penetration = penetrationOnAxis(one, two, axis, toCenter);
		
		if(penetration < 0)
			return false;
		
		if(penetration < smallestPenetration)
		{
			smallestPenetration = penetration;
			smallestCase = index;
		}
		return true;
	}
	
	/** This method is called when we know that a vertex from box two is in contact with box one. */
	public static void fillPointFaceBoxBox(CollisionBox one, CollisionBox two, Vector3 toCenter,
			CollisionData data, int best, float pen)
	{
		//Contact contact = data.m_contacts;
		Contact contact = new Contact();
		
		/** We know which axis the collision is on (i.e. best),
    	* but we need to work out which of the two faces on this axis. */
		Vector3 normal = one.getAxis(best);
		if(one.getAxis(best).dotProduct(toCenter) > 0)
			normal.multiplyAndSet(-1);
		
		/** Work out which vertex of box two we're colliding with.
		 * Using toCenter doesn't work! */
		Vector3 vertex = new Vector3(two.m_halfSize);
		if(two.getAxis(0).dotProduct(normal) < 0) vertex.x = -vertex.x;
		if(two.getAxis(1).dotProduct(normal) < 0) vertex.y = -vertex.y;
		if(two.getAxis(2).dotProduct(normal) < 0) vertex.z = -vertex.z;
		
		/** Create contact data */
		contact.m_contactNormal = normal;
		contact.m_penetration = pen;
		contact.m_contactPoint = two.getTransform().multiplyByVector(vertex);
		contact.setBodyData(one.m_body, two.m_body, data.m_friction, data.m_restitution);
		
		data.addContact(contact);
	}
	
	public static Vector3 contactPoint(
			Vector3 pOne, Vector3 dOne, float oneSize,
			Vector3 pTwo, Vector3 dTwo, float twoSize,
			/**If this is true, and the contact point is outside
			 * the edge (in the case of an edge-face contact) then
			 * we use one's midpoint, otherwise we use two's. */
			boolean useOne)
	{
		Vector3 toSt, cOne, cTwo;
		float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
		float denom, mua, mub;
		
		smOne = dOne.squareMagnitude();
		smTwo = dTwo.squareMagnitude();
		dpOneTwo = dTwo.dotProduct(dOne);
		
		toSt = pOne.subtract(pTwo);
		dpStaOne = dOne.dotProduct(toSt);
		dpStaTwo = dTwo.dotProduct(toSt);
		
		denom = smOne * smTwo - dpOneTwo * dpOneTwo;
		
		/**Zero denominator indicates parallel lines*/
		if(Math.abs(denom) < .0001){
			if(useOne)
				return pOne;
			else
				return pTwo;
		}
		mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
		mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;
		
		/** If either of the edges has the nearest point out
		 * of bounds, then the edges aren't crossed, we have
		 * an edge-face contact. Our point is on the edge, which
		 * we know from the useOne parameter. */
		if(mua > oneSize || mua < -oneSize || mub > twoSize || mub < -twoSize){
			if(useOne)
				return pOne;
			else
				return pTwo;
		}
		else
		{
			cOne = pOne.add(dOne.multiply(mua));
			cTwo = pTwo.add(dTwo.multiply(mub));
			
			Vector3 temp1 = cOne.multiply(.5f);
			Vector3 temp2 = cTwo.multiply(.5f);
			return temp1.add(temp2);
		}
	}
}
